#include "pub_tf.hpp"



void callbackImu(const sensor_msgs::ImuConstPtr &msg_imu)
{   
    std::lock_guard<std::mutex> lock1(imu_lock);
    imuQueue.push_back(*msg_imu);
    imu_buf.emplace(msg_imu);
     std::cout << "imuQueue.size(): " << imuQueue.size() << std::endl;
}

void callbackImage(const sensor_msgs::ImageConstPtr &msg_image)
{   
    // -----------buffer存值的时候需要上锁--------
    // --------方式1 lock() unlock()上锁和解锁--------------
    image_lock.lock();
    image_buf.push(msg_image);
    std::cout << image_buf.size() << std::endl;
    image_lock.unlock();

    // // --------方式2 lock_guard上锁-------------
    // std::lock_guard<std::mutex> lock2(image_lock);
    // double img_stamp = msg_image->header.stamp.toSec();
    // cv::Mat img = cv_bridge::toCvShare(msg_image, "bgr8")->image;
    // // std::make_pair组成对
    // imageQueue.push_back(std::make_pair(img_stamp, img));
    // // std::cout << imageQueue.size() << std::endl;
}

void imuProcess()
{   
    while(1)
    {
        // std::lock_guard<std::mutex> lock1(imu_lock);
        // std::lock_guard<std::mutex> lock2(image_lock);
        

        if(!imuQueue.empty())
        {   
            imu_lock.lock();
            imuQueue.pop_back();
            imu_buf.pop();
            std::cout << "imu_buf.size(): " << imu_buf.size() << std::endl;
            imu_lock.unlock();  
        }
        // 等待时间
        std::chrono::milliseconds dura(10);  // 毫秒
        std::this_thread::sleep_for(dura);   // 当前线程暂定执行dura毫秒

        // 取出激光点云队列中最早的一帧
        // std::move 是 C++11 中的一个函数，它的作用是将一个左值强制转化为右值引用。
        // 右值引用是一种可以绑定到临时对象的引用，它可以实现移动语义，即将一个对象的资源转移给另一个对象，而不是进行复制。
        // std::move 可以将任何的左值/右值都转化为右值引用，但是它并不会真的移动任何东西，只是改变了对象的值类别1。
        // imageLast = std::move(imageQueue.front().second);
        // timeImageLast = std::move(imageQueue.front().first);
        // 第2帧时间戳
        // timeImageCur = std::move(imageQueue.front().first);

        // imu队列为空,如果imu队列第一帧数据时间戳和队列最后一帧时间戳在两帧图像时间之内, 就不处理
        // if (imuQueue.empty() || imuQueue.front().header.stamp.toSec() > timeImageLast
        //     || imuQueue.back().header.stamp.toSec() < timeImageCur)
        // {
        //     ROS_DEBUG("Waiting for IMU data ...");
        //     return;
        // }
    }
}

void imageProcess()
{   
    while(1)
    {   
        cv::Mat image;
        std_msgs::Header header;
        double time = 0;

        if(!image_buf.empty())
        {
             // -----------去buffer取值的时候需要上锁--------
            image_lock.lock();
            time = image_buf.front()->header.stamp.toSec();
            header = image_buf.front()->header;
            image = cv_bridge::toCvShare(image_buf.front(), "bgr8")->image;
            image_buf.pop();
            image_lock.unlock();
            
            if(!image.empty())
            {
                // cv::imshow("view", image);
                // cv::waitKey(10);
                std::cout << "11111111" << std::endl;
            }
                
        }
        {
            std::cout << "0000000" << std::endl;
        }
        std::chrono::milliseconds dura(20);  // 毫秒
        std::this_thread::sleep_for(dura);   // 当前线程暂定执行dura毫秒
    }
   
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "pubtf_node");
    ros::NodeHandle n;

    // 入队
    sub_imu = n.subscribe("/imu/data", 1000, &callbackImu);
    sub_image = n.subscribe("/camera/rgb/image_raw", 1000, &callbackImage);
    
    // imageProcess出队速度比较快
    std::thread t1(imuProcess);
    std::thread t2(imageProcess);
    ros::spin();
    // ros::MultiThreadedSpinner(3);
    return 0;
}